/*
 * UAVLeader.h
 *
 *  Created on: Oct 26, 2016
 *      Author: throne
 */

#ifndef PX4_OPERATOR_SRC_OPTITRACK_UAV_OPERATOR_UAV_LEADER_H_
#define PX4_OPERATOR_SRC_OPTITRACK_UAV_OPERATOR_UAV_LEADER_H_

#include "optitrack_uav_operator.h"
#include <geometry_msgs/TwistStamped.h>
#include <string>
#include "../display_planner/DisPlayPlanner.h"
#include <fstream>
#include <iostream>
#include <string>
class UAVLeader {
public:
enum class Mission
{
	DISARM,
	FORCE_DISARM,
	ARMED_STANDBY,
	TAKEOFF,
	INIT_POSITION,
	INIT_POSITION1,
	INIT_POSITION2,
	LOITER,
	ROUNDING,
	FOLLOW,
	LAND,
	TAKEOFF_V2,
	DISPLAY,
	GOTHROUGH,
};

public:
	static const int MAX_UAV_NUM = 5;
	const double SAFETY_DISTANCE = 1.2;

private:
	bool inited; //currenttly unused
	bool busy;	//currenttly unused
	Mission mission;
	double time;
	double current_mission_time_left;
	double current_mission_elapse_time;
	double current_mission_full_time;

	int uav_num;
	OptitrackUAVOperator uav_operators[MAX_UAV_NUM];


	ros::Subscriber keyboard_sub;
	ros::Publisher  Key_mission_pub;

	bool reset_all_bias;
	//ros::Subscriber pose_set_sub = nh.subscribe<geometry_msgs::PoseStamped>
	//            ("keyboard_command", 10, keyboard_cb);
public:
	bool all_disarmed;
	bool all_armed;
	bool all_in_air;
	bool all_connected;

	//
	float rotate_modify_v;
	float liner_modify_v;

	float rotate_modify_r;//radius
	float vertical_modify_d;
	float x_modify_d;
	float y_modify_d;

	//
	bool is_running;
	ifstream path_ifs[MAX_UAV_NUM];

public:
	UAVLeader();
	virtual ~UAVLeader();

public:
	bool init(ros::NodeHandle& nh, int uav_num_, string& rigidbody_name_header, string& mavros_namespace_header);
	void send_uav_pose();
	void send_reset_bias_cmd();
	bool set_mission(Mission set_mission);
	bool command_uavs(double timestamp);
	double get_mission_execute_time(Mission req_mission);

	bool is_busy();
	Mission current_mission();
	std::string mission_to_string(Mission req_mission);
	void update_uav_status();

	void set_offboard();
	void spinonce();

	//zy
	void keyboard_callback(const geometry_msgs::TwistStamped::ConstPtr& msg);
	//void Displayplan();

};

#endif /* PX4_OPERATOR_SRC_OPTITRACK_UAV_OPERATOR_UAV_LEADER_H_ */
